﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Research.Kinect.Nui;

namespace KinectUserManager
{
    public enum UserAssertionState
    {
        Kinect,
        Obstacle,
        TiltDeficient,
        UserCompleted,
        UserCliped,
        Proximity
    }
    public enum KinectScanerState
    {
        InActive,
        Scaning,
        Complete,
        Adaptated
    }
    //delegado
    public delegate void NewUserEventHandler(object sender, NewUserEventArgs e);
    public delegate void SecondUserEventHandler(object sender, SecondUserFoundEventArgs e);
    public delegate void UserClippedEventHandler(object sender, UserClippedEventArgs e);
    public delegate void UserLostEventHandler(object sender, UserLostEventArgs e);
    public delegate void MainUserChangeEventHandler(object sender, MainUserChangedEventArgs e);
    public delegate void UserCompletedEventHandler(object sender, UserCompletedEventArgs e);
    public delegate void ProximityEventHandler(object sender, ProximityEventArgs e);
    //public delegate void VisionObstacleEventHandler(object sender, VisionObstacleEventArgs e);
    public class UserAsserter
    {
        public Runtime Kinect{ get; private set;}
        public int nTrackedUsers { get; private set;}
        public int firstUserID { get; private set;}
        public int[,] DepthDistance { get; private set;}
        public Dictionary<JointID, VectorXYZ> PlayerOneJoints = new Dictionary<JointID, VectorXYZ>();
        public Dictionary<JointID, VectorXYZ> PlayerTwoJoints = new Dictionary<JointID, VectorXYZ>();
        public SkeletonQuality skellQuality { get; private set; }
        public float Modulo;
        private VectorXYZ _mouseHand;
        public VectorXYZ Floor { get; private set; }
        public TimeSpan startScanTime{get; private set;}
        public TimeSpan lastUserLost { get; private set;}
        public KinectScanerState cameraState { get; private set; }
        public int adaptedCamarAngle { get; private set; }
        //eventos
        public event NewUserEventHandler NewUserFound;
        public event SecondUserEventHandler SecondUserFound;
        public event UserClippedEventHandler UserClipped;
        public event UserLostEventHandler UserLost;
        public event MainUserChangeEventHandler MainUserChanged;
        public event UserCompletedEventHandler UserCompleted;
        public event ProximityEventHandler ProximityRised;
        UserAssertionState userAssertionState;

        protected virtual void OnProximityRised(ProximityEventArgs e)
        {
            if (ProximityRised != null)
            {
                ProximityRised(this, e);
                userAssertionState = UserAssertionState.Proximity;
            }
        }

        protected virtual void OnUserCompleted(UserCompletedEventArgs e)
        {
            if (UserCompleted != null)
            {
                UserCompleted(this, e);
                userAssertionState = UserAssertionState.UserCompleted;
            }
        }

        protected virtual void OnNewUserFound(NewUserEventArgs e)
        {
            if (NewUserFound != null)
            {
                NewUserFound(this, e);
                userAssertionState = UserAssertionState.UserCompleted;
            }
        }
        protected virtual void OnSecondUserFound(SecondUserFoundEventArgs e)
        {
            if (SecondUserFound != null)
                SecondUserFound(this, e);
        }
        protected virtual void OnUserClipped(UserClippedEventArgs e)
        {
            if (UserClipped != null)
            {
                UserClipped(this, e);
                userAssertionState = UserAssertionState.UserCliped;
            }
        }
        //falta implementar

        protected virtual void OnUserLost(UserLostEventArgs e)
        {
            if (UserLost != null)
                UserLost(this, e);
            this.lastUserLost = e.LossTime;
            this.firstUserID = -1;
            this.cameraState = KinectScanerState.InActive;
        }
        protected virtual void OnMainUserChanged(MainUserChangedEventArgs e)
        {
            if (MainUserChanged != null)
                MainUserChanged(this, e);
        }
        /// <summary>
        /// this class need a Runtime, inicialized and with the depth stream opened
        /// </summary>
        public UserAsserter(Runtime kinect)
        {
            Kinect = kinect;
            kinect.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(kinect_SkeletonFrameReady);
            kinect.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(kinect_DepthFrameReady);
            this._mouseHand = new VectorXYZ() { X = 0, Y = 0, Z = 0 };
            this.firstUserID = -1;
            this.Modulo = 10;
            //this.Kinect.NuiCamera.ElevationAngle = 0;
            DateTime d=DateTime.Now;
            this.startScanTime = new TimeSpan(d.Hour, d.Minute, d.Second);
            this.lastUserLost = new TimeSpan(d.Hour, d.Minute, d.Second);
            this.cameraState = KinectScanerState.InActive;
        }

        public VectorXY getMouseHand(int Width, int Height){

            float depthX, depthY;
            float originX, originY;
            Vector Hand;
            Vector Shoulder;
            //si ma dreta aixecada mana
            if (TrakedSkeletonManager.TrakedSkelletons != null && TrakedSkeletonManager.TrakedSkelletons.Length>0)
            {
                if (TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HandRight].Position.Y > TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HipCenter].Position.Y || TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HandLeft].Position.Y < TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HipCenter].Position.Y)
                {
                    Hand = TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HandRight].Position;
                    Shoulder = TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.ShoulderRight].Position;
                }
                //sino mana la esquerra si esta aixecad
                else
                {
                    Hand = TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HandLeft].Position;
                    Shoulder = TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.ShoulderLeft].Position;
                }
            }
            else
            {
                return new VectorXY() { X = 0, Y = 0 };
            }

            Kinect.SkeletonEngine.SkeletonToDepthImage(Hand, out depthX, out depthY);
            Kinect.SkeletonEngine.SkeletonToDepthImage(Shoulder, out originX, out originY);
            Vector head = TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.Head].Position;
            float headX, headY;
            //Kinect.SkeletonEngine.SkeletonToDepthImage(rShoulder, out headX, out headY);
            Kinect.SkeletonEngine.SkeletonToDepthImage(head, out headX, out headY);
            //todos los float sacados del skeleton depth engine ya devuelven un valor entre  0 i 1
            double preadaptationX = 3 * (((double)depthX*10000)- ((double)originX*10000));
            double preadaptationY = 5 * (((double)depthY*10000) - ((double)headY)*10000);
            double adaptationX = 0.5 + (preadaptationX/10000);
            double adaptationY = 0.5 + (preadaptationY/10000);
            adaptationX *= Width;
            adaptationY *= Height;
            VectorXY handXY = new VectorXY() { X = (int)(adaptationX), Y = (int)(adaptationY) };
            if (handXY.X < 1)
            {
                handXY.X = 1;
            }
            if (handXY.Y < 1)
            {
                handXY.Y = 1;
            }
            if (handXY.X > Width-1)
            {
                handXY.X = Width-1;
            }
            if (handXY.Y > Height-1)
            {
                handXY.Y = Height-1;
            }
            return handXY;
            
        }
        private void kinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            
            //En el frame anteriror cuantos skelletons habia?
            int lastNSkelletons;
            int lastUserPlayerID = this.firstUserID;
           
            if (TrakedSkeletonManager.TrakedSkelletons != null)
            {
                //guado el anterior numero de esqueletos
                lastNSkelletons = TrakedSkeletonManager.TrakedSkelletons.Length;
                SkeletonFrame skeletonFrame = e.SkeletonFrame;
                //reinici la entrada de esceletos
                List<SkeletonData> skells = new List<SkeletonData>();
                foreach (SkeletonData data in skeletonFrame.Skeletons)
                {
                    if (SkeletonTrackingState.Tracked == data.TrackingState)
                    {
                        if (data != null)
                            skells.Add(data);
                    }
                }
                //me guardo los usuarios trakeados en la clase estatica
                if (TrakedSkeletonManager.TrakedSkelletons.Length < 1 && skells.Count > 0)
                {
                    OnNewUserFound(new NewUserEventArgs(skells[0].TrackingID));
                }
                TrakedSkeletonManager.OrderPlayerSkeletons(skells, this.firstUserID);
                //actualizo el numero de esqueletos guardados
                nTrackedUsers = TrakedSkeletonManager.TrakedSkelletons.Length;
                if (nTrackedUsers > 0&&TrakedSkeletonManager.TrakedSkelletons[0].Joints.Count>0)
                {
                    if (TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.Head].Position.Z < 1.5f)
                    {
                        OnProximityRised(new ProximityEventArgs(TrakedSkeletonManager.TrakedSkelletons[0].TrackingID));
                    }
                }
                if (lastNSkelletons == 0 && TrakedSkeletonManager.TrakedSkelletons.Length > 0)
                {
                    OnNewUserFound(new NewUserEventArgs(TrakedSkeletonManager.TrakedSkelletons[0].TrackingID));
                    if (cameraState == KinectScanerState.Scaning)
                    {
                        //aqui adaptacion para el escaneo
                        //aqui no entra nunca
                        if (TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.Head].TrackingState == JointTrackingState.Tracked)
                        {
                            Vector v = TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HipCenter].Position;
                            KinectScaner.HipCenter = new VectorXYZ() { X = v.X, Y = v.Y, Z = v.Z };
                            adaptedCamarAngle = KinectScaner.FigureInclination(Kinect.NuiCamera.ElevationAngle);

                            cameraState = KinectScanerState.Complete;
                        }                        
                    }
                }
                else if (cameraState == KinectScanerState.Scaning)
                {
                    adaptedCamarAngle = 0;
                    cameraState = KinectScanerState.Complete;
                }
                //si son menos de 2 esqueletos
                if (nTrackedUsers < 2)
                {
                    this.PlayerTwoJoints = new Dictionary<JointID, VectorXYZ>();
                    if (nTrackedUsers < 1)
                    {
                        this.PlayerOneJoints = new Dictionary<JointID, VectorXYZ>();
                        this.firstUserID = -1;
                    }
                    else
                    {
                        firstUserID = TrakedSkeletonManager.TrakedSkelletons[0].TrackingID;
                    }
                }
                //si havia un uusario y ahora hay mas dispara el evento
                else if (lastNSkelletons > 0 && TrakedSkeletonManager.TrakedSkelletons.Length > lastNSkelletons)
                {
                    
                    OnSecondUserFound(new SecondUserFoundEventArgs(TrakedSkeletonManager.TrakedSkelletons[0].TrackingID, TrakedSkeletonManager.TrakedSkelletons[1].TrackingID));
                }
                //Sirve para ver si te has salido de la pantalla
                if (nTrackedUsers> 0)
                {
                    //Si antes estabas en buena posicion y ahora no lo estas lanzas el evento
                    if (TrakedSkeletonManager.TrakedSkelletons[0].Quality != 0 && skellQuality == 0 && this.cameraState!=KinectScanerState.Scaning &&this.cameraState!=KinectScanerState.Complete)
                    {
                        if (TrakedSkeletonManager.TrakedSkelletons[0].Quality == SkeletonQuality.ClippedLeft || TrakedSkeletonManager.TrakedSkelletons[0].Quality == SkeletonQuality.ClippedRight || TrakedSkeletonManager.TrakedSkelletons[0].Quality == SkeletonQuality.ClippedTop)
                        {
                            OnUserClipped(new UserClippedEventArgs(TrakedSkeletonManager.TrakedSkelletons[0].Quality));
                        }
                    }
                    else if (TrakedSkeletonManager.TrakedSkelletons[0].Quality == 0 || TrakedSkeletonManager.TrakedSkelletons[0].Quality == SkeletonQuality.ClippedBottom && userAssertionState == UserAssertionState.UserCliped)
                    {
                        OnUserCompleted(new UserCompletedEventArgs(TrakedSkeletonManager.TrakedSkelletons[0].TrackingID));
                        
                    }
                    
                    //averigua la distanccia codo cabeza para ver cuando se levant ala mano
                    if (this.PlayerOneJoints.Count > 0)
                    {
                        this.Modulo = this.PlayerOneJoints[JointID.ElbowRight].Y - this.PlayerOneJoints[JointID.Head].Y;
                        //actualiza la posicion del raton
                        float x = TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HandRight].Position.X;
                        float y = TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HandRight].Position.Y;
                        float z = TrakedSkeletonManager.TrakedSkelletons[0].Joints[JointID.HandRight].Position.Z;
                        this._mouseHand = new VectorXYZ() { X = x, Y = y, Z = z };
                    }
                }
                //mira si alguien ha levantado la mano y cambia usuario
                this.assertFirstPlayer();
                //rellena los esqueletos
                for (int i = 0; i < nTrackedUsers; i++)
                {
                    if (TrakedSkeletonManager.TrakedSkelletons[i] != null)
                        fillJointDictionary(i);
                }
                //si el usuario ha cambiado o si hay dos i sale el primero
                if (lastUserPlayerID != -1 && firstUserID != -1 && lastUserPlayerID != firstUserID)
                {
                    this.OnMainUserChanged(new MainUserChangedEventArgs(firstUserID, lastUserPlayerID));
                }
                
                //si desaparece el usuario
                if (lastNSkelletons > 0 && TrakedSkeletonManager.TrakedSkelletons.Length < 1)
                {
                    OnUserLost(new UserLostEventArgs());
                }
            }
            else
            {
                lastNSkelletons = 0;
                TrakedSkeletonManager.OrderPlayerSkeletons(new List<SkeletonData>(), -1);
            }
            
        }
        
        private void kinect_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            PlanarImage pImage=e.ImageFrame.Image;
            int width = pImage.Width;
            int height = pImage.Height;

            byte[] rawBytes = pImage.Bits;
            //aqui pillo la distancia y admeas me la dejo ordenadita en coordenadas x y para que sea mas facil
            int [,] distance = new int[width,height];
            
            for (int y = 0; y < height; y++)
            {
                //aqui hago el mirror horizontal  de las distancias para que cuadre con el skeleton space de la pantalla
                for (int x = width - 1; x > -1; x--)
                //for (int x =0; x <width; x++)
                {
                    int n=((y*width)+((width-1)-x))*2;
                    distance[x,y] = (rawBytes[n] | rawBytes[n + 1] << 8);
                }
            }
            
            //aqui implementar el escaneo lo mas importante es el tiempo pa que kinecdt no se bloqueee
            DateTime now = DateTime.Now;
            TimeSpan t = new TimeSpan(now.Hour, now.Minute, now.Second);
            if (t.TotalSeconds - startScanTime.TotalSeconds > 2 && t.TotalSeconds - this.lastUserLost.TotalSeconds > 2)
            {
                if (firstUserID == -1)
                {

                    if (cameraState==KinectScanerState.Complete)
                    {
                        Kinect.NuiCamera.ElevationAngle = adaptedCamarAngle;
                        cameraState = KinectScanerState.InActive;
                        startScanTime = t;
                    }
                    else if (this.userAssertionState != UserAssertionState.Proximity && KinectScaner.checkDistance(this.DepthDistance, distance))
                    {
                        //Kinect.NuiCamera.ElevationAngle = Camera.ElevationMaximum;
                        //startScanTime = t;
                        //cameraState = KinectScanerState.Scaning;
                    }


                }

                else if (cameraState == KinectScanerState.Complete)
                {
                    if (adaptedCamarAngle > 25)
                        adaptedCamarAngle = 25;
                    Kinect.NuiCamera.ElevationAngle = adaptedCamarAngle;
                    cameraState = KinectScanerState.Adaptated;
                }
                
            }
            this.DepthDistance = distance;
        }
        private void fillJointDictionary(int index)
        {
            
            foreach (Joint jnt in TrakedSkeletonManager.TrakedSkelletons[index].Joints)
            {
                #region oldStuff
                //aqui podem veure si realment hi ha traking o nomes suposo que hi es la kinect
                if (jnt.TrackingState == JointTrackingState.Inferred)
                {

                }
                //si es el primer usuario
                if (index == 0)
                {
                    if (PlayerOneJoints.Count > 0)
                    {
                        PlayerOneJoints[jnt.ID] = new VectorXYZ() { X = jnt.Position.X, Y = jnt.Position.Y, Z = jnt.Position.Z };

                    }
                    else
                        PlayerOneJoints.Add(jnt.ID, new VectorXYZ() { X = jnt.Position.X, Y = jnt.Position.Y, Z = jnt.Position.Z });
                    
                    
                    
                }
                //si es el segundo usuario
                if (index == 1)
                {
                    if(PlayerTwoJoints.Count > 0)
                        PlayerTwoJoints[jnt.ID] = new VectorXYZ() { X = jnt.Position.X, Y = jnt.Position.Y, Z = jnt.Position.Z };
                    else
                        PlayerTwoJoints.Add(jnt.ID, new VectorXYZ() { X = jnt.Position.X, Y = jnt.Position.Y, Z = jnt.Position.Z });
                }
                #endregion oldStuff

            }
                    
            
        }
                
        private void assertFirstPlayer()
        {
            if (TrakedSkeletonManager.TrakedSkelletons.Length > 1&&TrakedSkeletonManager.TrakedSkelletons[1].Quality==0)
            {
                //primero mirar el recalculo y luego reasignar.
                SkeletonData skellTwo = TrakedSkeletonManager.TrakedSkelletons[1];
                float modulo = skellTwo.Joints[JointID.ElbowRight].Position.Y - skellTwo.Joints[JointID.Head].Position.Y;
                this.Modulo = modulo;
                if (modulo < 0)
                {
                    modulo *= -1;
                }
                if (modulo<0.2f)
                {
                    
                    this.firstUserID = TrakedSkeletonManager.TrakedSkelletons[1].TrackingID;
                }
            }
        }
    }
}
